from struct import pack
from tabnanny import verbose

from sympy import true
import launch
import launch.event_handlers
import launch.launch_description_sources
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os

import launch_ros.parameter_descriptions

# 这个文件将机器人加载到了gazebo中

def generate_launch_description():
    # 获取默认urdf路径
    urdf_package_path = get_package_share_directory('dofbot')
    defualt_xacro_path = os.path.join(urdf_package_path,'urdf','dofbot_arm.xacro')
    # defualt_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')
    defualt_gazebo_world_path = os.path.join(urdf_package_path,'world','dofbot_arm_gazebo.world')
    # 声明一个urdf的目录的参数方便修改
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(defualt_xacro_path),description='加载的模型文件路径'
    )
    
    # 通过文件路径获取内容，并转换为参数值对象，以供传入 robot_state_publisher
    substitutions_command_result = launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration(variable_name='model',default=defualt_xacro_path)])


    robot_description_value = launch_ros.parameter_descriptions.ParameterValue(substitutions_command_result,value_type=str)


    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description':robot_description_value}]
    )

    action_robot_gripper_orchestrator_node = launch_ros.actions.Node(
        package='dofbot',
        executable='gripper_orchestrator_node',
        parameters=[{
            'use_sim_time': True,
        }]
    )

    arm_control_node = launch_ros.actions.Node(
        package='dofbot',
        executable='arm_control_node',
         parameters=[{
            'use_sim_time': True,
        }]
    )

    # 用于执行运行gazebo的launch文件
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',defualt_gazebo_world_path),('verbose','true'),('use_sim_time','true')]
    )

    # 用于将机器人加载到gazebo中
    action_spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-topic','/robot_description','-entity','dofbot_arm']
    )

    #  启动controller_manager - 这是关键！
    action_controller_manager = launch_ros.actions.Node(
        package='controller_manager',
        executable='ros2_control_node',
        output='screen',
        parameters=[os.path.join(urdf_package_path, 'config', 'ros2_controllers.yaml')]
    )

    # 加载并启动joint_state_publisher 控制器，但是需要在加载玩机器人之后再运行
    action_load_joint_state_publisher = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller dofbot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )

    action_load_arm_controller = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller arm_controller --set-state active'.split(' '),
        output='screen'
    )

    action_load_host_gripper_controller = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller host_gripper_controller --set-state active'.split(' '),
        output='screen'
    )

    action_load_follow_gripper_controller = launch.actions.ExecuteProcess(
        cmd='ros2 control load_controller follow_gripper_controller --set-state active'.split(' '),
        output='screen'
    )
    

    

    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher,
        # action_joint_state_publisher,
        action_launch_gazebo,
        action_spawn_entity,
        action_controller_manager,
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity, # 在加载完机器人之后再加载并启动控制器
                on_exit=[action_load_joint_state_publisher],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_publisher, # 在加载完机器人之后再加载并启动控制器
                on_exit=[action_load_arm_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessExit(
                target_action=action_load_arm_controller, # 在加载完机器人之后再加载并启动控制器
                on_exit=[action_load_host_gripper_controller],
            )
        ),
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessExit(
                target_action=action_load_host_gripper_controller, # 在加载完机器人之后再加载并启动控制器
                on_exit=[action_load_follow_gripper_controller],
            )
        ),
        # 夹爪协调者节点发布
        launch.actions.RegisterEventHandler(
            event_handler = launch.event_handlers.OnProcessExit(
                target_action=action_load_follow_gripper_controller, 
                on_exit=[action_robot_gripper_orchestrator_node,arm_control_node],
            )
        ),
        
    ])


